# import cv2
# import matplotlib.pyplot as plt

# # 读取PGM文件
# image = cv2.imread(f'maps/map06041133.pgm', cv2.IMREAD_GRAYSCALE)
import numpy as np
import cv2
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan, Imu
from rclpy.node import Node
from geometry_msgs.msg import Point
import rclpy
import yaml
import os
script_dir = os.path.dirname(os.path.abspath(__file__))
image_path = os.path.join(script_dir, 'maps', 'map06041133.pgm')
param_path = os.path.join(script_dir, 'maps', 'map06041133.yaml')
print(image_path)
with open(param_path, 'r') as file:
    params = yaml.safe_load(file)
    print(params['resolution'])


class LaserScanSubscriber(Node):

    def __init__(self):
        super().__init__('laser_scan_subscriber')
        self.subscription = self.create_subscription(
            LaserScan,
            '/scan',  # 雷达话题名称，根据实际情况修改
            self.listener_callback,
            10
        )
        self.odom_subscription = self.create_subscription(
            Odometry,
            '/odom',  # 里程计话题名称
            self.odom_callback,
            10
        )
        self.imu_subscription = self.create_subscription(
            Imu,
            '/imu',  # IMU 话题名称
            self.imu_callback,
            10
        )
        self.odometer = Point()
        self.imu_angle = 0
        self.odometer.x = 10.0
        self.odometer.y = 10.0
        # self.odometer.x = float(params['origin'][0])
        # self.odometer.y = float(params['origin'][1])
        print(self.odometer.x)
        print(self.odometer.y)
        # 加载背景图像
        self.image = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
        if self.image is None:
            self.get_logger().error("Error: Could not load the background image.")
            return

        # 将灰度图像转换为 BGR 格式以便绘制彩色点
        self.image_bgr = cv2.cvtColor(self.image, cv2.COLOR_GRAY2BGR)

    def listener_callback(self, msg):
        # 将雷达数据转换为点云
        angle_min = msg.angle_min
        angle_max = msg.angle_max
        angle_increment = msg.angle_increment
        ranges = msg.ranges

        # 假设图像中心是 (width/2, height/2)
        height, width = self.image_bgr.shape[:2]
        center_x = self.odometer.x*20
        center_y = self.odometer.y*20

        # 清除之前的点云
        self.image_bgr = cv2.cvtColor(self.image, cv2.COLOR_GRAY2BGR)

        # 绘制点云
        for i, range_val in enumerate(ranges):
            if range_val < msg.range_max and range_val > msg.range_min:
                angle = angle_min + i * angle_increment+self.imu_angle
                x = int(range_val*20 * np.cos(angle)+ center_x) 
                y = int(range_val*20 * np.sin(angle)+ center_y) 

                # 确保点在图像范围内
                if 0 <= x < width and 0 <= y < height:
                    cv2.circle(self.image_bgr, (x, y), 2,
                               (0, 0, 255), -1)  # 绘制红色点

        # 显示结果
        cv2.imshow('Laser Scan', self.image_bgr)
        cv2.waitKey(1)

    def odom_callback(self, msg):
        pass

    def imu_callback(self, msg):
        # 更新方向（根据 IMU 数据）
        # 通常需要集成加速度计和陀螺仪数据来更新方向
        pass


def main(args=None):
    rclpy.init(args=args)

    laser_scan_subscriber = LaserScanSubscriber()

    try:
        rclpy.spin(laser_scan_subscriber)
    except KeyboardInterrupt:
        pass

    # 清理
    laser_scan_subscriber.destroy_node()
    rclpy.shutdown()
    cv2.destroyAllWindows()


if __name__ == '__main__':
    main()
